/*************************************************************************
*
*	controls.c
*
*  Copyright 2009 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module provides functions which abstract rudder and throttle
*    commands from the low-level hardware interface.
*
*
**************************************************************************
*
*  version 1.0.3, 4/10/09, SBS
*    - add THROTTLE/RUDDER_MAX/MIN
*
*  version 1.0.2, 3/6/09, SBS
*    - change port numbers on sd84 to reflect actual boat wiring
*
*  version 1.0.1, 2/22/09, SBS
*    - add #ifdef DEBUG around logprint
*
*  version 1.0, 1/9/09, SBS
*
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @file 
*  @ingroup plat_engineering
*  @brief
*  This module provides functions which abstract rudder and throttle
*    commands from the low-level hardware interface.
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>

//#include "ssa.h"
#include "types.h"
#include "logfile.h"

#include "sd84.h"

#include "controls.h"

#include "readconfigfile.h"


#define SERVO_PORT_THROTTLE 4
#define SERVO_PORT_RUDDER 2

#define THROTTLE_MAX 100
#define THROTTLE_MIN -100

#define RUDDER_MAX 65
#define RUDDER_MIN -65

rudder_calibration ruddercal;

/********************************************************
*
* controls_SetThrottle()
*/
/**
* @brief
* Set throttle servo position (-100 to 100)
*
*
********************************************************/
BYTE controls_SetThrottle(BYTE value)
{

	if((value > THROTTLE_MAX)||(value < THROTTLE_MIN)){
		#ifdef DEBUG
		logfile_LogLevelPrint(LOGLEVEL_CRITICAL
				,"Invalid parameter in controls_SetThrottle()");
		#endif
		printf("Invalid parameter in controls_SetThrottle()\n");
		return(-1);
	}

	sd84_set_servo(SERVO_PORT_THROTTLE,value);

	return(0);
}


/********************************************************
*
* controls_SetRudder()
*/
/**
* @brief
* Set rudder servo position (-100 to 100)
*
*
********************************************************/
BYTE controls_SetRudder(BYTE value)
{

/*	if((value > RUDDER_MAX)||(value < RUDDER_MIN)){
		#ifdef DEBUG
		logfile_LogLevelPrint(LOGLEVEL_CRITICAL
				,"Invalid parameter in controls_SetRudder()");
		#endif
		printf("Invalid parameter in controls_SetRudder()\n");
		return(-1);
	}
*/

	controls_SetCalibratedRudder(value);
	
	return(0);
}


/********************************************************
*
* controls_GetRudder()
*/
/**
* @brief
* Retrieve current rudder servo position
*
*
********************************************************/
BYTE controls_GetRudder()
{

	sd84_get_servo(SERVO_PORT_RUDDER);
	
	return(0);

}


/********************************************************
*
* controls_Init()
*/
/**
* @brief
* Initialize servos
*
*
********************************************************/
void controls_Init(void)
{

	sd84_set_mode_servo(SERVO_PORT_RUDDER);
	sd84_set_mode_servo(SERVO_PORT_THROTTLE);
	
	ruddercal.polarity = readconfig_int("calibration.cfg", "rudder", "polarity", 1);
	ruddercal.centeroffset = readconfig_int("calibration.cfg", "rudder", "centeroffset", 0);
	ruddercal.positivescalefactor = readconfig_int("calibration.cfg", "rudder", "positivescalefactor", 100);
	ruddercal.negativescalefactor = readconfig_int("calibration.cfg", "rudder", "negativescalefactor", 100);

	return;
}

/********************************************************
*
* controls_SetCalibratedRudder()
*/
/**
* @brief
* sets the calibrated values for the rudder
*
*
********************************************************/
void controls_SetCalibratedRudder(BYTE value)
{
	BYTE rudder = value;
	BYTE calrudder;
	
	calrudder = rudder * ruddercal.polarity;
	calrudder = calrudder + ruddercal.centeroffset;
	
	if(calrudder>0)
		calrudder *= ruddercal.positivescalefactor/100;  
	else
		calrudder *= -ruddercal.negativescalefactor/100;
		
	sd84_set_servo(SERVO_PORT_RUDDER,calrudder);
	
	return;		
	
}


